#include "trigger_response/EventServe.h"

handle::handle(const ros::NodeHandle &nh,const ros::NodeHandle &nh_private,const string& nodeName)
    :nh_(nh),nh_private_(nh_private),Nodename_(nodeName)
{
    nh_private_.param("Thd1",Thd1_,Thd1_);// 小车感知事件范围的远近门限
    nh_private_.param("Thd2",Thd2_,Thd2_);// 小车与无人机的距离远近门限
    nh_private_.param("Thd3",Thd3_,Thd3_);// 小车距离目标点的远近门限
    service_ = nh_.advertiseService("eventHandle",&handle::ServiceCallback,this);

    // // 创建服务客户端
    // ros::ServiceClient client = nh_.serviceClient<trigger_response::event>("eventHandle");
    // ros::service::waitForService("eventHandle");  // 等待服务可用


    // air_odom_sub = nh_.subscribe<nav_msgs::Odometry>("/air_bodyinworld/odom", 1, &handle::odomCallback, this);
    // std::cout << "xxxxxxxx" << std::endl;
    // car_odom_sub = nh_.subscribe<nav_msgs::Odometry>("/car_bodyinworld/odom", 1, &handle::odomCallback, this);
    robot_odom_sub = nh_.subscribe<nav_msgs::Odometry>("robot_1", 1, &handle::robotCallback, this);

    // list_service_sub = nh_.subscribe<std::int32_t>("list_trigger",1, std::bind(&handle::ListServiceCallback, &nh_, std::placeholders::_1));
    list_service_sub = nh_.subscribe<std_msgs::Int32>("list_trigger", 1, &handle::ListServiceCallback, this);

    // action_  = std::make_shared<trigger_response_action>(nh_);
    action_  = std::make_shared<trigger_response_action>(nh_, nh_private_);
    
}
template<typename T>
T getparam(const string& name,const T& defaultValue){ // name = Nodename+_eventCode
    T v;
    if(ros::param::get(name,v)){ // get parameter by name depend on ROS
        ROS_INFO_STREAM("Found parameter: "<<name<<",\tvalue: "<<v);
        return v;
    }else{
        ROS_WARN_STREAM("Cannot find value for parameter: "<<name<<",\tassigning default: "<<defaultValue);
        return defaultValue;//if the parameter haven't been set,it's value will return defaultValue.
    }
}

void handle::getRespose(string &eventCodeName,const short& smallType){
    switch(smallType){ 
        case 0:
            eventCodeName += "_000";
            break;
        case 1:
            eventCodeName += "_001";
            break;
        case 2:
            eventCodeName += "_010";
            break;
        case 3:
            eventCodeName += "_011";
            break;
        case 4:
            eventCodeName += "_100";
            break;
        case 5:
            eventCodeName += "_101";
            break;
        case 6:
            eventCodeName += "_110";
            break;
        case 7:
            eventCodeName += "_111";
            break;
    }
    respose_ = getparam<int>(eventCodeName,0);
}


bool handle::ServiceCallback(trigger_response::event::Request &req,
                            trigger_response::event::Response &res){
    short AGDis_bool = 0; //无人机/无人车的相对距离远,近判断 远为0 近为1
    short TGDis_bool = 0; //目标点/无人车的相对距离远,近判断 远为0 近为1,没有目标点,设置为1
    short EGDis_bool = 0; //事件/无人车的相对距离远,近判断 远为0 近为1,多个事件选择优先级高的,优先级从低到高:其他,人,车,火力点
    ROS_INFO("UavPos:(%lf,%lf,%lf)",req.UavPos[0],req.UavPos[1],req.UavPos[2]);
    ROS_INFO("UgvPos:(%lf,%lf,%lf)",req.UgvPos[0],req.UgvPos[1],req.UgvPos[2]);
    ROS_INFO("TargetPos:(%lf,%lf,%lf)",req.TargetPos[0],req.TargetPos[1],req.TargetPos[2]);
    ROS_INFO("EType:(%d,%d,%d,%d)",req.EType[0],req.EType[1],req.EType[2],req.EType[3]);
    ROS_INFO("eventLocx:(%lf,%lf,%lf,%lf)",req.eventLocx[0],req.eventLocx[1],req.eventLocx[2],req.eventLocx[3]);
    ROS_INFO("eventLocy:(%lf,%lf,%lf,%lf)",req.eventLocy[0],req.eventLocy[1],req.eventLocy[2],req.eventLocy[3]);
    ROS_INFO("eventLocz:(%lf,%lf,%lf,%lf)",req.eventLocz[0],req.eventLocz[1],req.eventLocz[2],req.eventLocz[3]);


    AGDis_ = sqrt(pow((req.UavPos[0]-req.UgvPos[0]),2)+pow((req.UavPos[1]-req.UgvPos[1]),2)
                +pow((req.UavPos[2]-req.UgvPos[2]),2));
    AGDis_bool = AGDis_>Thd2_?0:1;
    if((req.TargetPos[0]==req.UgvPos[0]&&req.TargetPos[1]==req.UgvPos[1]
        &&req.TargetPos[2])==req.UgvPos[3]){ // 目标点与无人车的坐标一致 表示没有目标点,小车静止
        TGDis_bool = 1;
    }else{
        TGDis_ = sqrt(pow((req.TargetPos[0]-req.UgvPos[0]),2)+pow((req.TargetPos[1]-req.UgvPos[1]),2)
                    +pow((req.TargetPos[2]-req.UgvPos[2]),2));
        TGDis_bool = TGDis_>Thd3_?0:1;
    }

    int type = req.EType[0]*1+req.EType[1]*2+req.EType[2]*4+req.EType[3]*8;
    if(type==0){
        return false;
    }
    ROS_INFO("type = %d",type);
    short smallType;
    string eventCodeName;
    eventCodeName.clear();
    switch(type){
        case 1:
            eventCodeName = Nodename_+ "/_1000";
            EGDis_ = sqrt(pow((req.eventLocx[0]-req.UgvPos[0]),2)+pow((req.eventLocy[0]-req.UgvPos[1]),2)
                +pow((req.eventLocz[0]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
            getRespose(eventCodeName,smallType);
        break;
        case 2:
            eventCodeName = Nodename_+ "/_0100";
            EGDis_ = sqrt(pow((req.eventLocx[1]-req.UgvPos[0]),2)+pow((req.eventLocy[1]-req.UgvPos[1]),2)
                +pow((req.eventLocz[1]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
            getRespose(eventCodeName,smallType);

        break;
        case 4:
            eventCodeName = Nodename_+ "/_0010";
            EGDis_ = sqrt(pow((req.eventLocx[2]-req.UgvPos[0]),2)+pow((req.eventLocy[2]-req.UgvPos[1]),2)
                +pow((req.eventLocz[2]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 8:
            eventCodeName = Nodename_+ "/_0001";
            EGDis_ = sqrt(pow((req.eventLocx[3]-req.UgvPos[0]),2)+pow((req.eventLocy[3]-req.UgvPos[1]),2)
                +pow((req.eventLocz[3]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 3:
            eventCodeName = Nodename_+ "/_1100";
            EGDis_ = sqrt(pow((req.eventLocx[1]-req.UgvPos[0]),2)+pow((req.eventLocy[1]-req.UgvPos[1]),2)
                +pow((req.eventLocz[1]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 5:
            eventCodeName = Nodename_+ "/_1010";
            EGDis_ = sqrt(pow((req.eventLocx[2]-req.UgvPos[0]),2)+pow((req.eventLocy[2]-req.UgvPos[1]),2)
                +pow((req.eventLocz[2]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 9:
            eventCodeName = Nodename_+ "/_1001";
            EGDis_ = sqrt(pow((req.eventLocx[3]-req.UgvPos[0]),2)+pow((req.eventLocy[3]-req.UgvPos[1]),2)
                +pow((req.eventLocz[3]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 6:
            eventCodeName = Nodename_+ "/_0110";
            EGDis_ = sqrt(pow((req.eventLocx[2]-req.UgvPos[0]),2)+pow((req.eventLocy[2]-req.UgvPos[1]),2)
                +pow((req.eventLocz[2]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 10:
            eventCodeName = Nodename_+ "/_0101";
            EGDis_ = sqrt(pow((req.eventLocx[3]-req.UgvPos[0]),2)+pow((req.eventLocy[3]-req.UgvPos[1]),2)
                +pow((req.eventLocz[3]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 12:
            eventCodeName = Nodename_+ "/_0011";
            EGDis_ = sqrt(pow((req.eventLocx[3]-req.UgvPos[0]),2)+pow((req.eventLocy[3]-req.UgvPos[1]),2)
                +pow((req.eventLocz[3]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 7:
            eventCodeName = Nodename_+ "/_1110";
            EGDis_ = sqrt(pow((req.eventLocx[2]-req.UgvPos[0]),2)+pow((req.eventLocy[2]-req.UgvPos[1]),2)
                +pow((req.eventLocz[2]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 11:
            eventCodeName = Nodename_+ "/_1011";
            EGDis_ = sqrt(pow((req.eventLocx[3]-req.UgvPos[0]),2)+pow((req.eventLocy[3]-req.UgvPos[1]),2)
                +pow((req.eventLocz[3]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 13:
            eventCodeName = Nodename_+ "/_1011";
            EGDis_ = sqrt(pow((req.eventLocx[3]-req.UgvPos[0]),2)+pow((req.eventLocy[3]-req.UgvPos[1]),2)
                +pow((req.eventLocz[3]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
            getRespose(eventCodeName,smallType);

        break;
        case 14:
            eventCodeName = Nodename_+ "/_0111";
            EGDis_ = sqrt(pow((req.eventLocx[3]-req.UgvPos[0]),2)+pow((req.eventLocy[3]-req.UgvPos[1]),2)
                +pow((req.eventLocz[3]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
             getRespose(eventCodeName,smallType);

        break;
        case 15:
            eventCodeName = Nodename_+ "/_1111";
            EGDis_ = sqrt(pow((req.eventLocx[3]-req.UgvPos[0]),2)+pow((req.eventLocy[3]-req.UgvPos[1]),2)
                +pow((req.eventLocz[3]-req.UgvPos[2]),2));
            EGDis_bool = EGDis_>Thd1_?0:1;
            smallType = AGDis_bool*4+EGDis_bool*2+TGDis_bool*1;
            getRespose(eventCodeName,smallType);

        break;
    }
    ROS_INFO("smallType = %d",smallType);
    // TODO:这里应该加入对于response的值的处理
    res.response = respose_;
    // 这里的targetpos应该是目标物的位置
    Eigen::Vector3d ugvpos, uavpos, target_pos; 
    ugvpos = Eigen::Vector3d(req.UavPos[0],req.UavPos[1],req.UavPos[2]);
    uavpos = Eigen::Vector3d(req.UgvPos[0],req.UgvPos[1],req.UgvPos[2]);
    target_pos = Eigen::Vector3d(req.TargetPos[0],req.TargetPos[1],req.TargetPos[2]);
    action_->trigger_response(ugvpos, uavpos, target_pos,respose_ );
    return true;
}

void handle::ListServiceCallback(const std_msgs::Int32::ConstPtr& msg){
    Eigen::Vector3d ugvpos, uavpos, target_pos; 
    if (msg->data == 0){
        respose_ = 0;
        ugvpos = {0,0,0};
        uavpos = {0,0,0};
        target_pos = robot_pose;
        action_->trigger_response(ugvpos, uavpos, target_pos,respose_ );
    }
    if (msg->data == 1){
        respose_ = 1;
        ugvpos = {0,0,0};
        uavpos = {0,0,0};
        target_pos = robot_pose;
        action_->trigger_response(ugvpos, uavpos, target_pos,respose_ );
    }
    if (msg->data == 2){
        respose_ = 2;
        ugvpos = {0,0,0};
        uavpos = {0,0,0};
        target_pos = robot_pose;
        action_->trigger_response(ugvpos, uavpos, target_pos,respose_ );
    }
    if (msg->data == 3){
        respose_ = 3;
        ugvpos = {0,0,0};
        uavpos = {0,0,0};
        target_pos = robot_pose;
        action_->trigger_response(ugvpos, uavpos, target_pos,respose_ );
    }
    if (msg->data == 4){
        respose_ = 4;
        ugvpos = {0,0,0};
        uavpos = {0,0,0};
        target_pos = robot_pose;
        action_->trigger_response(ugvpos, uavpos, target_pos,respose_ );
    }

}

void handle::robotCallback(const nav_msgs::Odometry::ConstPtr& msg){
    // robot_pose = {static_cast<float>(msg->pose.pose.position.x), static_cast<float>(msg->pose.pose.position.y), static_cast<float>(msg->pose.pose.position.z)};
    robot_pose = {-11.5, 25, 0};

}


